#ifndef VISIBLECORRECTOR
#define VISIBLECORRECTOR
#include <cstring>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
#include "common.h"
using namespace cv;
using namespace std;
class CVisibleCorrector {
 private:
  int ini = 0;

 public:
  CVisibleCorrector(/* args */) {}
  ~CVisibleCorrector() {}
  void Run(void);
  vector<Point2f> DetectCornerPoints(Mat &img, bool is_draw = false, bool with_template = false);
  Mat FeatureMatching(const Mat& img, const vector<Point2f>& corner_points);
  Mat GetROI(const Mat& img, const vector<Point2f>& points);
  Rect DetectTargetArea(const Mat& img);
  Rect DetectTargetByTemplate(const Mat& img, const Mat& template_img);
  vector<Point2f> DetectTargetAndCorners(const Mat& img, const Mat& template_img, bool is_draw=false);
  bool TryDetectCornerPoints(vector<Point2f>& pts_dst, Mat &src_img, bool is_draw=false) ;
  void SortPoints(std::vector<cv::Point>& points);
};

vector<Point2f> CVisibleCorrector::DetectCornerPoints(Mat &img, bool is_draw, bool with_template) {
  static int num = 0;
  num++;
  std::string img_Path = "";
  Mat src_img = img.clone();
  
  vector<Point2f> pts_dst;
  
  // 判断是否为灰度图，如果不是则转换
  if(src_img.channels() != 1) {
    cvtColor(src_img, src_img, COLOR_BGR2GRAY);  /////将图像转为灰度图像/////
  }
  
  // 预处理1：直方图均衡化处理
  // equalizeHist(src_img, src_img);
  // 预处理2：自动白平衡
  // double minVal, maxVal;
  // minMaxLoc(src_img, &minVal, &maxVal);
  // // 计算缩放因子
  // double scale = 255.0 / (maxVal - minVal);
  // // 应用线性变换进行白平衡
  // src_img.convertTo(src_img, CV_8U, scale, -minVal * scale);
  // 预处理3：增加对比度
  // Mat enhanced_img;
  // double alpha = 1.5;  // 增加对比度
  // double beta = 0;    // 保持亮度不变
  // convertScaleAbs(src_img, enhanced_img, alpha, beta);
  // src_img = enhanced_img.clone();
  
  int mean = 0;
  int low_average = 0x3fff;
  int mean_high = 0;
  int mean_low = 0;
  int sum_high = 0;
  int sum_low = 0;
  int value = 0;

  /*
  /////////////////////////////求算术平均值作为分割阈值,不太合理////////////////////////////////
  for (int i = 0; i < src_img.rows; i++) {
    for (int j = 50; j < src_img.cols - 50; j++) {
      value = src_img.at<uchar>(i, j);
      if (value > low_average) {
        mean_high += value;
        sum_high += 1;
      } else {
        mean_low = value;
        sum_low = 1;
      }
    }
  }
  if ((sum_high == 0) || (sum_low == 0)) {
    low_average = (mean_high / (sum_high + 1) + mean_low / (sum_low + 1));
  } else
    low_average = (mean_high / sum_high + mean_low / sum_low) / 2;

  ///////////////////由平均值求大和小两种灰度值，然后求平均值作为分割阈值////////////////
  mean_high = 0;
  mean_low = 0;
  sum_high = 0;
  sum_low = 0;
  for (int i = 0; i < src_img.rows; i++) {
    for (int j = 50; j < src_img.cols - 50; j++) {
      value = src_img.at<uchar>(i, j);
      if (value > low_average) {
        mean_high = mean_high + value;
        sum_high = sum_high + 1;
      } else {
        mean_low = mean_low + value;
        sum_low = sum_low + 1;
      }
    }
  }

  low_average = (mean_high / sum_high + mean_low / sum_low) / 2;
  // low_average = low_average + 5;
  */

  if(with_template){
    // 计算中心区域的范围
    int center_x = src_img.cols * 0.5;
    int center_y = src_img.rows * 0.5;
    int roi_width = (int)(src_img.cols * 0.5);
    int roi_height = (int)(src_img.rows * 0.5);
    int roi_x = center_x - roi_width / 2;
    int roi_y = center_y - roi_height / 2;
    
    // 提取中心区域
    Mat center_roi = src_img(Rect(roi_x, roi_y, roi_width, roi_height));
    
    // 对中心区域使用大津法计算阈值
    double otsu_thresh = cv::threshold(center_roi, center_roi, 0, 255, THRESH_BINARY | THRESH_OTSU);
    // std::cout << "Otsu threshold from center region: " << otsu_thresh << std::endl;
    
    // 使用计算出的阈值对整个图像进行二值化
    Mat test_img;
    for (int i = 0; i < 5; ++i) { 
      double test_otsu_thresh = otsu_thresh - i*5;
      if (test_otsu_thresh < 0) break;
      cv::threshold(src_img, test_img, test_otsu_thresh, 255, THRESH_BINARY);
      if (TryDetectCornerPoints(pts_dst, test_img, is_draw)) break;
    }
    
  }
  else{
    // 直接使用大津法进行二值化
    Mat test_img;
    cv::threshold(src_img, test_img, low_average, 255, THRESH_OTSU);
    TryDetectCornerPoints(pts_dst, test_img, is_draw);
  }
  ///////////////////////////////////////////////////////////
  if(is_draw&& pts_dst.size()==8) {
    for(int i=0; i<8; ++i) {
      circle(img, pts_dst[i], 7, Scalar(0, 0, 255), 2, 8);  // 在角点上画圆///
      cv::putText(img, to_string(i+1), pts_dst[i], cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 255, 255), 1, 8, 0);  /// 在角点上标1~8数字///MMMMM
      char res_txt[100];
      sprintf(res_txt, "(%d,%d)", int(pts_dst[i].x), int(pts_dst[i].y));
      string str(res_txt);
      cv::putText(img, str, Point(pts_dst[i].x + 30, pts_dst[i].y),
                  cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 255, 255), 1, 8,
                  0);  ////在角点上标坐标位置////
    }
  }
  if(is_draw){
    img_Path = "dstImg" + std::to_string(num) + ".jpg";
    imwrite(img_Path, img);
  }
  
  return pts_dst;
}

bool CVisibleCorrector::TryDetectCornerPoints(vector<Point2f>& pts_dst, Mat &src_img, bool is_draw) {
  int data_temp_x[8], data_temp_y[8];
  int data_x[8], data_y[8];
  std::string img_Path = "";
  bool no_area_counter = true;
  bool no_8_vertex_counter = true;
  static int num = 0;
  num++;
  if (is_draw) {
    img_Path = "src_img" + std::to_string(num) + ".jpg";
    imwrite(img_Path, src_img);
  }
  cv::Mat element(5, 5, CV_8U, cv::Scalar(1));
  cv::bitwise_not(src_img, src_img);      ////黑色变白色////
  if (is_draw) {
    img_Path = "src_bit_not_img" + std::to_string(num) + ".jpg";
    imwrite(img_Path, src_img);
  }
  
  cv::dilate(src_img, src_img, element);  ////膨胀////
  if (is_draw) {
    img_Path = "src_dilate_img" + std::to_string(num) + ".jpg";
    imwrite(img_Path, src_img);
  }
  
  cv::erode(src_img, src_img, element);   ////腐蚀////
  if (is_draw) {
    img_Path = "src_erode_img" + std::to_string(num) + ".jpg";
    imwrite(img_Path, src_img);
  }
  
  
  vector<vector<Point>> contours;
  vector<Vec4i> hierarcy;
  findContours(src_img, contours, hierarcy, RETR_LIST, CHAIN_APPROX_NONE);

    vector<vector<Point>> contours_poly(contours.size());  // 用于存放折线点集
    int max_area = 0;
    int max_poly_area = 0;
    int min_poly_area = 1280*720;
    int contour_idx = -1;
    bool is_valid_convex = false;
    for (int i = 0; i < contours.size(); i++) {
        double area1 = contourArea(contours[i]);
        if (area1 > max_area) {
            max_area = area1;
        }
        if (area1 > 20000) {
            no_area_counter = false;
            double eps = contours[i].size() * 0.025;  // 0.01--0.03935
            approxPolyDP(Mat(contours[i]), contours_poly[i], eps, true);
            if (contours_poly[i].size() == 8) {
                no_8_vertex_counter = false;
                int area = cv::contourArea(contours_poly[i]);
                bool is_start_from_corner = false;
                for (auto pt : contours_poly[i]) {
                    if (pt.x < 2 || pt.y < 2) {
                        is_start_from_corner = true;
                        continue;
                    }
                }
                bool is_i_valid_convex = false;
                {
                  SortPoints(contours_poly[i]);
                  vector<Point> pts = contours_poly[i];
                  double dis12 = abs(pts[0].x - pts[1].x) + abs(pts[0].y - pts[1].y);
                  double dis34 = abs(pts[2].x - pts[3].x) + abs(pts[2].y - pts[3].y);
                  double dis56 = abs(pts[4].x - pts[5].x) + abs(pts[4].y - pts[5].y);
                  double dis78 = abs(pts[6].x - pts[7].x) + abs(pts[6].y - pts[7].y);
                  bool is_order_valid_12 = std::min(pts[0].y, pts[1].y) < std::max(pts[2].y, pts[3].y);
                  bool is_order_valid_23 = std::min(pts[2].y, pts[3].y) < std::max(pts[4].y, pts[5].y);
                  bool is_order_valid_34 = std::min(pts[4].y, pts[5].y) < std::max(pts[6].y, pts[7].y);
                  if(dis12 < dis56 && dis12 < dis78 && dis34 < dis56 && dis34 && dis78 && is_order_valid_12 && is_order_valid_23 && is_order_valid_34) {
                    is_i_valid_convex = true;
                  }
                  std::cout << "is_valid: " << is_i_valid_convex << " dis: "<< dis12 << " " << dis34 << " " << dis56 << " " << dis78 << std::endl;
                  std::cout << pts[0] << " " << pts[1] << " " << pts[2] << " " << pts[3] << " " 
                    << pts[4] << " " << pts[5] << " " << pts[6] << " " << pts[7] << std::endl;
                }
                if ((area < min_poly_area) && !is_start_from_corner && is_i_valid_convex) {
                    min_poly_area = area;
                    contour_idx = i;
                    is_valid_convex = true;
                }
            }
        }
    }
    
  if(is_valid_convex) {
    for (int i=0; i<8; ++i) {
      Point2f pt(contours_poly[contour_idx][i].x, contours_poly[contour_idx][i].y);
      pts_dst.push_back(pt);
    }
  }
  if (no_area_counter) {
      cout << "Warning: No area detected! max_area: " << max_area << endl;
  }
  if (no_8_vertex_counter) {
      cout << "Warning: No 8 vertex detected!" << endl;
  }
  return is_valid_convex;
}

void CVisibleCorrector::SortPoints(std::vector<cv::Point>& points) {
    if (points.size() != 8) return;

    // 找到最长边
    int dis_max = 0;
    int idx = 0;
    for (int j = 0; j < 7; j++) {
        int d = abs((points[j].x - points[j + 1].x)) +
                abs((points[j].y - points[j + 1].y));
        if (d > dis_max) {
            dis_max = d;
            idx = j;
        }
    }

    // 确定起始点
    int num_0 = (idx == 4) ? 1 : 0;
    
    // 重新排序点
    std::vector<cv::Point> temp_points(8);
    for (int j = 0; j < 8; j++) {
        int id_j = (j + num_0) % 8;
        temp_points[j] = points[id_j];
    }

    // 按x坐标排序
    for (int i = 0; i < 8; i++) {
        for (int j = 0; j + 1 < 8 - i; j++) {
            if (temp_points[j].x > temp_points[j + 1].x) {
                std::swap(temp_points[j], temp_points[j + 1]);
            }
        }
    }

    // 根据y坐标调整顺序
    if (temp_points[2].y <= temp_points[3].y) {
        points[0] = temp_points[2];
        points[2] = temp_points[3];
    } else {
        points[0] = temp_points[3];
        points[2] = temp_points[2];
    }

    if (temp_points[0].y <= temp_points[1].y) {
        points[4] = temp_points[0];
        points[6] = temp_points[1];
    } else {
        points[4] = temp_points[1];
        points[6] = temp_points[0];
    }

    if (temp_points[4].y <= temp_points[5].y) {
        points[1] = temp_points[4];
        points[3] = temp_points[5];
    } else {
        points[1] = temp_points[5];
        points[3] = temp_points[4];
    }

    if (temp_points[6].y <= temp_points[7].y) {
        points[5] = temp_points[6];
        points[7] = temp_points[7];
    } else {
        points[5] = temp_points[7];
        points[7] = temp_points[6];
    }
}

void CVisibleCorrector::Run(void) {
  double data;
  double data_src[16], data_dst[16];

  int num = 0;
  std::string img_path = "";

  vector<Point2f> point_src, point_dst;
  vector<Point2f> p_src, p_dst;
  string strPath;
  FILE *fp;

  if (ini == 1) {
    /////////////////读入可见光特征点//////////////////////
    strPath = "E:\\correction\\V_H" + std::to_string(num) + ".txt";
    fp = fopen(strPath.c_str(), "r+");

    for (int i = 0; i < 16; i++) {
      data_src[i] = data;
    }
    fclose(fp);
    for (int i = 0; i < 8; i++) {
      point_src.push_back(Point2f(data_src[i * 2 + 0], data_src[i * 2 + 1]));
    }

    /////////////////读入红外特征点/////////////////////
    strPath = "E:\\correction\\I" + std::to_string(num) + ".txt";
    // fp = fopen(strPath.c_str(), "r+");
    // fp = fopen(".\\I1.txt", "r+");
    // if (fp) {
    //   for (int i = 0; i < 16; i++) {
    //     fscanf(fp, "%lf\n", &data);
    //     data_dst[i] = data;
    //   }

    //   for (int i = 0; i < 8; i++) {
    //     point_dst.push_back(Point2f(data_dst[i * 2 + 0], data_dst[i * 2 +
    //     1]));
    //   }
    //   fclose(fp);
    // }

    ////////////////////生成转换矩阵H////////////////////////
    strPath = "E:\\correction\\H_H" + std::to_string(num) + ".txt";
    // fp = fopen(strPath.c_str(), "w+");
    // fp = fopen(".\\H1.txt", "w+");
    Mat H = findHomography(point_src, point_dst);
    // for (int i = 0; i < 3; i++)
    //   for (int j = 0; j < 3; j++) {
    //     double da = H.at<double>(i, j);
    //     fprintf(fp, "%lf\n", da);
    //   }
    // fclose(fp);

    strPath = "P" + std::to_string(num) + ".txt";
    fp = fopen(strPath.c_str(), "r+");
    // fp = fopen(".\\P1.txt", "r+");
    if (fp) {
      for (int i = 0; i < 16; i++) {
        fscanf(fp, "%lf\n", &data);
        data_dst[i] = data;
      }
      fclose(fp);
    } else {
    }

    for (int i = 0; i < 8; i++) {
      p_dst.push_back(Point2f(data_dst[i * 2 + 0], data_dst[i * 2 + 1]));
    }

    strPath = "E:\\correction\\corr_BulletData" + std::to_string(num) + ".txt";
    fp = fopen(strPath.c_str(), "w+");
    H = findHomography(point_dst, p_dst);
    for (int i = 0; i < 3; i++)
      for (int j = 0; j < 3; j++) {
        double da = H.at<double>(i, j);
        fprintf(fp, "%lf\n", da);
      }
    fclose(fp);
    ini = 0;
  }
}

Mat CVisibleCorrector::GetROI(const Mat& img, const vector<Point2f>& points) {
    // 检查输入参数
    if(img.empty()) {
        cout << "Error: Input image is empty!" << endl;
        return Mat();
    }
    
    if(points.empty()) {
        cout << "Error: No points provided for ROI calculation!" << endl;
        return Mat();
    }
    
    // 计算外接矩形
    float xmin = points[0].x, xmax = points[0].x;
    float ymin = points[0].y, ymax = points[0].y;
    
    for(const auto& pt : points) {
        if(pt.x < 0 || pt.y < 0 || pt.x >= img.cols || pt.y >= img.rows) {
            cout << "Warning: Point (" << pt.x << "," << pt.y << ") is outside image bounds!" << endl;
            continue;
        }
        xmin = min(xmin, pt.x);
        xmax = max(xmax, pt.x);
        ymin = min(ymin, pt.y);
        ymax = max(ymax, pt.y);
    }
    
    // 扩大ROI区域，确保包含完整特征
    int margin = 20;
    xmin = max(0.0f, xmin - margin);
    ymin = max(0.0f, ymin - margin);
    xmax = min((float)img.cols - 1, xmax + margin);
    ymax = min((float)img.rows - 1, ymax + margin);
    
    // 验证ROI区域的有效性
    int width = (int)(xmax - xmin);
    int height = (int)(ymax - ymin);
    
    if(width <= 0 || height <= 0) {
        cout << "Error: Invalid ROI dimensions: " << width << "x" << height << endl;
        return Mat();
    }
    
    // 确保ROI不会超出图像边界
    if(xmin + width > img.cols || ymin + height > img.rows) {
        cout << "Error: ROI exceeds image bounds!" << endl;
        return Mat();
    }
    
    try {
        return img(Rect((int)xmin, (int)ymin, width, height));
    } catch(const cv::Exception& e) {
        cout << "Error creating ROI: " << e.what() << endl;
        return Mat();
    }
}

Mat CVisibleCorrector::FeatureMatching(const Mat& img, const vector<Point2f>& corner_points) {
    // 获取ROI区域
    Mat roi_img = GetROI(img, corner_points);
    // Mat roi_img = imread("standard.jpg");
    
    // 读取标准图像
    Mat standard_img = imread("standard.jpg");
    if(standard_img.empty()) {
        cout << "Error: Cannot read standard image!" << endl;
        return Mat();
    }
    
    // 将ROI图像缩放到与标准图像相近的尺寸
    Mat resized_roi;
    resize(roi_img, resized_roi, standard_img.size(), 0, 0, cv::INTER_LINEAR);
    
    // 转换为灰度图
    Mat gray_roi, gray_standard;
    cvtColor(resized_roi, gray_roi, COLOR_BGR2GRAY);
    cvtColor(standard_img, gray_standard, COLOR_BGR2GRAY);
    
    // 图像预处理：增强对比度
    equalizeHist(gray_roi, gray_roi);
    equalizeHist(gray_standard, gray_standard);
    
    // 创建SIFT特征检测器，调整参数以增加特征点
    Ptr<SIFT> sift = SIFT::create(
        0,     // nfeatures: 检测的最大特征点数量，0表示不限制
        3,     // nOctaveLayers: 每组的层数
        0.04,  // contrastThreshold: 对比度阈值，降低以检测更多特征
        10,    // edgeThreshold: 边缘阈值
        1.6    // sigma: 高斯滤波系数
    );
    
    // 检测特征点和描述符
    vector<KeyPoint> keypoints_roi, keypoints_standard;
    Mat descriptors_roi, descriptors_standard;
    sift->detectAndCompute(gray_roi, Mat(), keypoints_roi, descriptors_roi);
    sift->detectAndCompute(gray_standard, Mat(), keypoints_standard, descriptors_standard);
    
    // 创建特征匹配器
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create(DescriptorMatcher::FLANNBASED);
    
    // 双向特征匹配
    vector<vector<DMatch>> knn_matches1, knn_matches2;
    matcher->knnMatch(descriptors_roi, descriptors_standard, knn_matches1, 2);
    matcher->knnMatch(descriptors_standard, descriptors_roi, knn_matches2, 2);
    
    // 应用Lowe's ratio test筛选好的匹配点，适当放宽阈值
    vector<DMatch> good_matches;
    vector<bool> roi_used(keypoints_roi.size(), false);
    vector<bool> standard_used(keypoints_standard.size(), false);
    
    // 第一步：应用比率测试
    vector<DMatch> matches1, matches2;
    for(size_t i = 0; i < knn_matches1.size(); i++) {
        if(knn_matches1[i][0].distance < 0.75 * knn_matches1[i][1].distance) {
            matches1.push_back(knn_matches1[i][0]);
        }
    }
    
    for(size_t i = 0; i < knn_matches2.size(); i++) {
        if(knn_matches2[i][0].distance < 0.75 * knn_matches2[i][1].distance) {
            matches2.push_back(knn_matches2[i][0]);
        }
    }
    
    cout << "Matches after ratio test - Forward: " << matches1.size() 
         << ", Backward: " << matches2.size() << endl;
    
    // 第二步：交叉验证
    for(const auto& match1 : matches1) {
        bool found_cross_match = false;
        for(const auto& match2 : matches2) {
            // 检查是否为相互最佳匹配
            if(match1.queryIdx == match2.trainIdx && 
               match1.trainIdx == match2.queryIdx) {
                // 确保这个匹配还没被使用过
                if(!roi_used[match1.queryIdx] && !standard_used[match1.trainIdx]) {
                    good_matches.push_back(match1);
                    roi_used[match1.queryIdx] = true;
                    standard_used[match1.trainIdx] = true;
                    found_cross_match = true;
                    break;
                }
            }
        }
    }
    
    // 获取匹配点对
    vector<Point2f> src_points, dst_points;
    for(const auto& match : good_matches) {
        src_points.push_back(keypoints_roi[match.queryIdx].pt);
        dst_points.push_back(keypoints_standard[match.trainIdx].pt);
    }

    // 使用RANSAC剔除错误匹配
    vector<uchar> inliers_mask;
    if(src_points.size() >= 4) {
        // findHomography内部使用RANSAC算法，同时返回内点掩码
        Mat H = findHomography(src_points, dst_points, RANSAC, 3.0, inliers_mask);
        
        // 根据RANSAC结果筛选匹配点
        vector<DMatch> ransac_matches;
        for(size_t i = 0; i < inliers_mask.size(); i++) {
            if(inliers_mask[i]) {
                ransac_matches.push_back(good_matches[i]);
            }
        }
        good_matches = ransac_matches;  // 更新good_matches为RANSAC筛选后的结果
        
        // 更新匹配点对
        src_points.clear();
        dst_points.clear();
        for(const auto& match : good_matches) {
            src_points.push_back(keypoints_roi[match.queryIdx].pt);
            dst_points.push_back(keypoints_standard[match.trainIdx].pt);
        }
    }
    
    // debug
    // 创建匹配结果图像
    Mat match_result;
    drawMatches(resized_roi, keypoints_roi, standard_img, keypoints_standard, good_matches, match_result,
                Scalar::all(-1), Scalar::all(-1), vector<char>(),
                DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
    
    // 保存匹配结果图像
    imwrite("match_res.jpg", match_result);

    // 计算单应性矩阵
    if(src_points.size() >= 4) {
        Mat H = findHomography(src_points, dst_points, RANSAC);
        return H;
    }
    
    // 保存灰度图像到当前文件夹
    imwrite("roi_gray.jpg", gray_roi);
    imwrite("standard_gray.jpg", gray_standard);
    return Mat();
}

Rect CVisibleCorrector::DetectTargetByTemplate(const Mat& img, const Mat& gray_template) {
    CTimer timer("DetectTargetByTemplate");
    timer.start();
    static int num = 0;
    num++;
    // 转换为灰度图
    Mat gray_img;
    cvtColor(img, gray_img, COLOR_BGR2GRAY);
    
    // 多尺度匹配
    double best_score = -1;
    double best_scale = 1.0;
    Rect best_rect;
    
    // 定义缩放范围，可以根据实际情况调整
    vector<double> scales = {0.5, 0.6, 0.75};
    
    // 保存所有尺度的匹配结果用于调试
    Mat debug_img = img.clone();
    int y_offset = 30;
    Mat best_result;

    for(double scale : scales) {
        // 缩放模板
        Mat scaled_template;
        resize(gray_template, scaled_template, Size(), scale, scale);
        
        // 如果缩放后的模板大于原图，跳过
        if(scaled_template.cols > gray_img.cols || scaled_template.rows > gray_img.rows) {
            cout << "Scale " << scale << " skipped: template too large" << endl;
            continue;
        }
        
        // 模板匹配
        Mat result;
        matchTemplate(gray_img, scaled_template, result, TM_CCOEFF_NORMED);
        
        // 找到最佳匹配位置
        double min_val, max_val;
        Point min_loc, max_loc;
        minMaxLoc(result, &min_val, &max_val, &min_loc, &max_loc);
        
        // 在调试图像上显示当前尺度的匹配分数
        // string score_text = "Scale " + to_string(scale) + ": " + to_string(max_val);
        // putText(img, score_text, Point(10, y_offset), 
        //         FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,255,0), 1);
        y_offset += 20;
        
        if(max_val > best_score) {
            best_score = max_val;
            best_scale = scale;
            best_rect = Rect(max_loc.x, max_loc.y, scaled_template.cols, scaled_template.rows);
            best_result = result;
        }
    }
    timer.end();
    // 如果找到好的匹配
    // if(best_score > 0.5) {  // 可以调整这个阈值
    //     // 在调试图像上绘制最佳匹配结果
    //     rectangle(img, best_rect, Scalar(0,255,0), 2);
    //     string best_text = "Best Scale: " + to_string(best_scale) + 
    //                       ", Score: " + to_string(best_score);
    //     putText(img, best_text, Point(10, y_offset), 
    //             FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0,255,0), 1);
        
    //     // 保存调试图像
    //     // std::string img_Path = "template_match_result" + std::to_string(num) + ".jpg";
    //     // imwrite(img_Path, img);
        
    // } else {
    //     cout << "Warning: No good match found (best score: " << best_score << ")" << endl;
    // }
    
    return best_rect;
}

vector<Point2f> CVisibleCorrector::DetectTargetAndCorners(const Mat& img, const Mat& template_img, bool is_draw) {
  static int num = 0;
  num++;  
    // 1. 使用模板匹配检测目标区域
    Rect target_rect = DetectTargetByTemplate(img, template_img);
    if(target_rect.empty()) {
        cout << "Failed to detect target area!" << endl;
        return vector<Point2f>();
    }
    
    // 2. 计算ROI大小
    int roi_size = (int)(max(target_rect.width, target_rect.height) * 1.5);
    
    // 3. 计算ROI中心点
    Point center(target_rect.x + target_rect.width/2, 
                target_rect.y + target_rect.height/2);
    
    // 4. 计算ROI边界
    int x1 = max(0, center.x - roi_size/2);
    int y1 = max(0, center.y - roi_size/2);
    int x2 = min(img.cols, center.x + roi_size/2);
    int y2 = min(img.rows, center.y + roi_size/2);
    
    // 5. 提取ROI
    Mat roi = img(Rect(x1, y1, x2-x1, y2-y1));
    
    // 6. 保存ROI用于调试
    if(is_draw) {
      std::string img_Path = "roi_for_corner_detection" + std::to_string(num) + ".jpg";
      imwrite(img_Path, roi);
    }
    
    // 7. 在ROI上进行角点检测
    vector<Point2f> corners = DetectCornerPoints(roi, is_draw, true);
    
    // 8. 将角点坐标转换回原图坐标系
    for(auto& corner : corners) {
        corner.x += x1;
        corner.y += y1;
    }
    
    // 9. 在原图上绘制检测结果
    if(is_draw){
      // 绘制ROI区域
      rectangle(img, Rect(x1, y1, x2-x1, y2-y1), Scalar(0,255,0), 2);
      // 绘制检测到的角点
      for(const auto& corner : corners) {
        circle(img, corner, 5, Scalar(0,0,255), -1);
      }
      std::string img_Path = "target_and_corners_result" + std::to_string(num) + ".jpg";
      imwrite(img_Path, img);
    }
    
    return corners;
}
#endif